/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package com.impulserobotics;
import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.Joystick;
import edu.wpi.first.wpilibj.RobotDrive;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.ADXL345_I2C;
/**
 *
 * @author Sahil Jain
 */
public class RobotDrivePlus extends RobotDrive{
    boolean driveBackward = false;
    boolean directionButtonPressed = false;
    boolean shootingMode = false;
    boolean balanceMode = false;
    Joystick curJoy;
    ADXL345_I2C accel;

    public RobotDrivePlus(int leftMotorChannel, int rightMotorChannel, Joystick joy, ADXL345_I2C acc) {
        super(leftMotorChannel, rightMotorChannel);
        curJoy = joy;
        accel = acc;
    }

    public void driveBackwards(boolean driveBackward) {
        this.driveBackward = driveBackward;
    }
        public void updateDriveDirection() {
                if (curJoy.getRawButton(Constants.DIRECTION_BUTTON) != directionButtonPressed) {
            directionButtonPressed = !directionButtonPressed;
            if (directionButtonPressed == false) {
                driveBackward = !driveBackward;
                
            }
            
        }
    }
    
   
    public void arcadeDrive(GenericHID stick) {
        // simply call the full-featured arcadeDrive with the appropriate values
        double stickY;
        double stickX = stick.getX();
        if(stick.getRawButton(Constants.BALANCE_BUTTON)){
            stickY = 0;
            stickX = 0;
            //use accel now
            double x = accel.getAccelerations().XAxis;
           // double y = accel.getAccelerations().YAxis;
            
                arcadeDrive(x, 0);
            
            
        }else{
        if(driveBackward){
            stickY = -stick.getY();
        }else{
            stickY = stick.getY();
        }
        if(shootingMode){
            stickY = 0;
        }

         arcadeDrive(stickY, stickX, false);
        }
        SmartDashboard.putDouble("driveX", stickX);
        SmartDashboard.putDouble("driveY", stickY);
    }
    
//        public void tankDrive(GenericHID leftStick, GenericHID rightStick) {
//        if (leftStick == null || rightStick == null) {
//            throw new NullPointerException("Null HID provided");
//        }
//        if(driveBackward){
//            tankDrive(-rightStick.getY(), -leftStick.getY());
//        }else{
//        tankDrive(leftStick.getY(), rightStick.getY());
//        }
//    }



     void computeMaxSpeed() {
          // determine if tank or arcade mode, based upon position of "Z" wheel on kit joystick
        if (curJoy.getButton(Joystick.ButtonType.kTrigger)) {
            setMaxOutput(5.0);
        } else {
            double speed = (-curJoy.getZ() + 1) / 2;
            setMaxOutput(speed);
        }
    }

    void update() {
        computeMaxSpeed();
        updateDriveDirection();
        arcadeDrive(curJoy);
    }

    void setShootingMode(boolean in) {
        shootingMode = in;
    }

    void setStick(Joystick joy) {
        curJoy = joy;
    }

}
